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SYSTEMS AND METHODS OF 

COORDINATION CONTROL FOR ROBOT 
MANIPULATION 

REFERENCE TO RELATED APPLICATIONS 5 

This application claims the benefit of priority of U.S. Pro- 
visional Application No. 61/115,461, titled “SYSTEM AND 
METHOD OF COORDINATION CONTROL FOR ROBOT 
MANIPULATION” and filed Nov. 17, 2008, and also claims 10 
the benefit of priority of U.S. Provisional Application No. 
61/115,468, titled “HUMAN-ROBOT MANIPULATION 
FOR COMPLEX OPERATIONS” and filed Nov. 17, 2008, 
both of which are hereby incorporated by reference in its 
entirety. 15 

FEDERALLY SPONSORED RESEARCH AND 
DEVELOPMENT 

This invention was made with Government support under 20 
contract no. N66001-07-M-1079 with the Space & Naval 
Warfare Systems Center of the U.S. Navy and contract 
NNC06CA27C with NASA Jet Propulsion Laboratory. The 
U.S. Government has certain rights in the invention. 

25 

FIELD 

The present application generally relates to systems and 
methods for controlling robotic apparatus such as, for 
example, manipulators, arms, and mobile bases. 30 

BACKGROUND 

Robotic apparatus, e.g., of the type making up remote 
autonomous agents, often act (or execute instructions) 35 
according to control software implementing one or more 
control algorithms. Those control algorithms transform input 
from, e.g., a user or sensors, into robotic action. 

Robotic systems are increasingly called upon to perform 
ever more complicated types of tasks. Such tasks often 40 
require complex coordination amongst various elements of a 
robotic system. However, coordinating control amongst sev- 
eral robotic elements (e.g., a moving ann, tool attached to the 
arm, mobile base, legs, and so on) has proven difficult and 
remains computationally intensive. Of particular difficulty is 45 
the coordination of simultaneous control (e.g., to effect 
simultaneous motion) of one or more arms with one or more 
mobile bases based on a desired motion of, for example, one 
element of the system. Further, user interfaces for controlling 
such robotic apparatus can be limited in the information they 50 
provide to a user as the user attempts to direct the action of the 
robot. 

Accordingly, there is a need for improved systems and 
methods for coordinating control of various elements making 
up a robotic apparatus, and for improved user interfaces for 55 
controlling robotic apparatus. 

SUMMARY 

Hie present application is generally directed to methods 60 
and systems for coordinated control of robotic systems, 
including coordinated control of robotic systems that include 
a mobile base and an articulating arm. In many cases, the 
systems and methods of the invention allow for simultaneous 
movement of the base and the arm so as to transition a robot 65 
from an initial state (e.g., characterized by an initial position 
and pose or angular orientation) to a desired final state. In 


2 

some implementations the base and the arm are modeled as a 
plurality of links or elements. A control vector is formed by 
concatenating components corresponding to the time rate of 
change of position of the link associated with the base (e.g., 
linear and angular velocities of the base) with the respective 
components corresponding to other links that are associated, 
e.g., with the arm. The optimal values of the components of 
the control vector(s) for motion in the next and subsequent 
time intervals can be determined by optimizing a merit func- 
tion, e.g., which can be a scalar function in many implemen- 
tations. Control signals based on the optimized values of the 
control vector components are applied to the base and one or 
more elements of the ann to cause motion. In many cases, the 
motion of the base and one or more elements of the arm is 
concurrent. 

In one aspect, a method is disclosed for controlling a 
robotic apparatus which includes a plurality of movable ele- 
ments coupled to one another via one or more joints defining 
one or more degrees of freedom of movement for each of the 
movable elements, at least one of the movable elements com- 
prising a mobile base for the apparatus. The control method 
can include receiving input signals indicating a desired posi- 
tion or motion constraint for at least one of the movable 
elements, and generating, based on the input signals, a plu- 
rality of control signals to control movement of the mobile 
base and at least one other movable element to achieve the 
desired motion by optimizing a pre-defined scalar function of 
the motion and positions of the movable elements and the 
mobile base. The method further includes applying the con- 
trol signals to the mobile base and the at least one other 
movable element to concurrently move the mobile base and 
the at least one other movable element to effect the desired 
motion. In some cases, the method can include applying the 
control signals to one or more actuators coupled to the one or 
more movable elements to effect the desired motion. 

In some embodiments, the plurality of movable elements 
included in the robotic apparatus can form or include a robotic 
ann(s) or mobile base(s). In some embodiments, at least one 
of the movable elements includes or represents an end effec- 
tor. The end effector can be a tool such as a gripper, a cutter, 
a saw, a haptic element, or a multi-fingered end effector. In 
many embodiments, at least one of the movable elements (in 
many cases, the mobile base) provides contact with the envi- 
ronment (e.g., the external environment) for the purpose of 
locomotion. The mobile base can include any of a wheel, a 
track, and a rail, or in other embodiments, any of a digging, 
climbing, and spelunking apparatus, and in other embodi- 
ments, any of a boat, submarine, and hovercraft, and in yet 
other embodiments, any of an airplane, helicopter, and 
lighter-than-air vehicle. The mobile base can have two or 
more degrees of freedom of movement comprising linear 
velocity and angular velocity. In some cases the mobile base 
can be constrained in its movement, e.g., for modeling a 
device operating on a track, for example, which may not be 
able to move laterally. In some cases, the movable elements of 
the robotic apparatus exhibit, collectively, at least seven 
degrees of freedom of movement. 

In some embodiments, the input signals can be generated 
by a user input device, and/or can be over any of audio signal, 
electromagnetic signal, or a computer network. In some 
embodiments, the method can include displaying (e.g., via a 
user display such as a computer screen, and in some cases 
simultaneously displaying) a plurality of views of the robotic 
apparatus pose, and allowing the user to control the robotic 
apparatus from each of the plurality of views with the user 
input device. As one skilled in the art will understand, other 
views, to which the user cannot switch to effect control pur- 
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poses, can additionally be included. The user input device can 
require only a single user action to switch control between the 
plurality of views. The single user action can include, for 
example, a mouseclick, a mouseover, a selection from a 
menu, a touch on a touch sensitive screen, a depression of a 5 
key or other tactile sensor, or a command-line entry (e.g., in a 
text -based interface). In some embodiments, the method can 
include displaying a plurality of views of the robotic appara- 
tus pose, allowing a user to control the robotic apparatus from 
each of the plurality of views, and automatically updating to 
each of plurality of the views based on a user’s control input 
in any one view. In some embodiments, the plurality of views 
can include rendered views. 

In some embodiments, the scalar function (also referred to 
as a merit function herein) can be associated with any of: 15 
kinetic energy associated with movement of the robotic appa- 
ratus, a gradient of potential energy associated with move- 
ment of the robotic apparatus, singularity avoidance, accu- 
racy, joint-limit avoidance, obstacle avoidance. The scalar 
function can signify a constraint on movement of at least one 20 
movable element. In some cases, the scalar function can be 
defined by: 


1 T T 25 

H - -Q 1 WQ + otq F. 

where q is a n-length vector representation of the positions 
of the one ormorejoints and the mobile base; with q being its 
time derivative (change in position over time); W is a matrix 3< 1 
function W(q); F is a vector function of configuration; and a 
is a scalar function of configuration. The parameters W, F and 
a can be selected to define a control strategy for controlling 
the robotic apparatus. 

In yet other embodiments, the generation of control signals 35 
in the control method can include determining control vectors 
(e.g., q) using the following equation: 


* Uj>J l-aNjFl 

where q is a n-length vector representation of the positions of 
the one or more joints and the mobile base; with q being its 45 
time derivative (change in position over time); V is a m-length 
vector representation of desired motion of at least one of the 
plurality of movable elements; J is an (mxn) manipulator 
Jacobian; N/As an nx(n-m) set of vectors defined such that 
JN/=0; W is a matrix function of q; F is a vector function of 50 
q; and a is a scalar function of q. In some embodiments, V can 
be specified (e.g., by a user) for several of, the majority of, or 
all of the plurality of movable elements. 

In another aspect, a robotic system is disclosed which 
includes a plurality of movable elements coupled to one 55 
another via one or more joints defining one or more degrees of 
freedom of movement for each of the movable elements, at 
least one of said movable elements comprising a mobile base. 
The robotic system can also include a user input device for 
receiving input signals indicating a desired position or motion 60 
constraint for at least one point on one of the movable ele- 
ments, and one or more processors coupled to the input device 
and generating a plurality of control signals to control move- 
ment of the mobile base and at least one other movable 
element to achieve the desired motion by optimizing a pre- 65 
defined scalar function of motion and positions of the mov- 
able elements and the mobile base. Hie robotic system can 
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further include one or more actuators coupled to the plurality 
of movable elements and to the one or more processors, the 
one or more actuators effecting the desired motion by con- 
currently moving the mobile base and the at least one other 
movable element in accordance with the control signals. 

A wide array of variations are possible. For example, in one 
embodiment, at least one of the movable elements can com- 
prise an end effector which includes any of a gripper, a cutter, 
a saw, a haptic element, a multi -fingered end effector. In some 
embodiments, the base can include any of a wheel, a track, 
and a rail. In some embodiments, the plurality of movable 
elements comprise a robotic arm and a mobile base. 

In yet further embodiments, the scalar function can be 
defined by: 


fj = -q T Wq + aq T F. 

where q is a n-length vector representation of the positions of 
the one or more joints and the mobile base; with q being its 
time derivative (change in position over time); W is a matrix 
function W(q); F is a vector function of configuration; and a 
is a scalar function of configuration. 

In some embodiments, the system can further include an 
operator control unit that (i) displays a plurality of views of 
the robotic apparatus pose (e.g., via a user display), and (ii) 
allows a user to control the robotic apparatus from each of the 
plurality of the views (e.g., via the aforementioned user input 
device, which can be incorporated into or part of the operator 
control unit). The operator control unit (e.g., via its software 
user interface) can require only a single user action to switch 
control between the plurality of views. In some embodiments, 
the system can include an operator control unit that (i) dis- 
plays a plurality of views of the robotic apparatus pose, (ii) 
allows a user to control the robotic apparatus from each of the 
plurality of views, and (iii) automatically updates each of the 
plurality of views based on a user’s control input in any one 
view. In some embodiments, the plurality of views can 
include a plurality of rendered views. 

In another aspect, a robotic system is disclosed which 
includes a plurality of movable elements coupled to one 
another via one ormorejoints defining one or more degrees of 
freedom of movement for each of the movable elements, at 
least one of said movable elements comprising a mobile base. 
The system can further include a user input device for receiv- 
ing input signals indicating a desired motion for at least one 
point on one of the movable elements, and one or more 
processors receiving the input signals and generating a plu- 
rality of control signals to control movement of the mobile 
base and at least one other movable element to achieve the 
desired motion. Further, the system can include one or more 
actuators coupled to the plurality of movable elements and to 
the one or more processors, the one or more actuators effect- 
ing the desired motion by concurrently moving the mobile 
base and the at least one other movable element in accordance 
with the control signals. In many embodiments, the one or 
more processors can generate the control signals using the 
following equation: 


rf-vi 

[Njw J L -aNjF\ 

where q is a n-length vector representation of the positions of 
the one or more joints and the mobile base; with q being its 
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time derivative; V is a 111-length vector representation of 
desired motion of at least one point of one of the plurality of 
movable elements; J is an (111x11) manipulator Jacobian; N/ds 
an nx(n-m) set of vectors defined such that JN/= 0 ; W is a 
matrix functi on W(q);F is the gradient of scalar function ,f(q); 5 
a is a scalar. In some embodiments, V can be specified (e.g., 
by a user) for several of, the majority of, or all of the plurality 
of movable elements. In many cases, the parameters W, F and 
a are user-selected to define a control strategy for controlling 
the robotic apparatus. Further, in some embodiments, the one 10 
or more processor can generate the control signals so as to 
optimize a merit function defined as: 

1 15 

fl = -q T Wq + q T Fa 

where p. represents a parameter to be optimized that reflects 
the control strategy. 20 

In some embodiments, the system can further include an 
operator control unit that (i) displays a plurality of views of 
the robotic apparatus pose (e.g., via a user display), and (ii) 
allows a user to control the robotic apparatus from each of the 
plurality of views (e.g., via a user input device). The operator 25 
control unit (e.g., via its software user interface) can require 
only a single user action to switch control between a plurality 
of the views. Further, in some embodiments, the system can 
include an operator control unit that (i) displays a plurality of 
views of the robotic apparatus pose, (ii) allows a user to 30 
control the robotic apparatus from each of the plurality of the 
views, and (iii) automatically updates each of the plurality of 
views based on a user’s control input in any one view. In some 
embodiments, the plurality of views can include a plurality of 
rendered views. 35 

In another aspect, a method is disclosed for controlling a 
robotic apparatus which includes a plurality of movable ele- 
ments coupled to one another via one or more joints defining 
one or more degrees of freedom of movement (and in some 
cases at least seven degrees of freedom of movement, and in 40 
other cases more than seven) for each of the movable ele- 
ments, with at least one of the movable elements comprising 
a mobile base for the apparatus. The control method can 
include input signals indicating a desired motion for at least 
one point on one of the movable elements, which can be 45 
treated as an end effector element. The method can further 
include determining, based on the desired motion of the at 
least one point on the end effector element, a plurality of 
control vectors representing position changes for the one or 
more joints and the mobile base to achieve the desired motion. 50 
The control vectors can represent time rate of position 
change, for example, and they can be determined so as to 
optimize a merit function associated with a user-desired con- 
trol strategy. The control vectors are applied to the mobile 
base and the at least one other movable element to concur- 55 
rently move the mobile base and the at least one other mov- 
able element and effect the desired motion. 

In some embodiments, the method can further include 
modeling the plurality of movable elements as a plurality of 
links, at least one link modeling the mobile base (“base link”), 60 
at least one link including at least one point for which a 
desired motion is sought (“end effector link”), and at least one 
link representing an aspect link, the movement of other links 
being determined in relation to the aspect link. 

In another aspect, a method is disclosed for controlling a 65 
robotic apparatus which includes a plurality of movable ele- 
ments coupled to one another via one or more joints defining 
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one or more degrees of freedom of movement for each of the 
movable elements, at least one of said movable elements 
comprising a mobile base for the apparatus. The control 
method can include receiving input signals indicating a 
desired motion for at least one point on one of the movable 
elements, and calculating, with a digital data processor, a 
plurality of control signals to control movement of the mobile 
base and at least one other movable element. Further, those 
control signals can be applied to the mobile base and the at 
least one other movable element to concurrently move the 
mobile base and the at least one other movable element and 
effect the desired motion. The calculation of the control sig- 
nals can use the equation: 

[NjW \ [-aNjFi 

where q is a n-length vector representation of the positions of 
the one or more joints and the mobile base; with q being its 
time derivative; V is a m-length vector representation of a 
desired motion of the plurality of movable elements; J is an 
(mxn) manipulator Jacobian; N/'is an nx(n-m) set of vectors 
defined such that JN/= 0 ; W is a matrix function W(q); F is 
the gradient of scalar function ;f(q); a is a scalar. The forego- 
ing equation can optimize the following merit function: 

F = \q 7 Wq + q T Fa 

where p represents a parameter to be optimized that reflects a 
control strategy. 

In another aspect, a robot is disclosed which includes a 
mobile base capable of moving in at least one dimension, an 
articulating arm coupled to the mobile base, and a control 
system for generating control signals for application to the 
base and the arm so as to move the base while concurrently 
changing a configuration of the arm. The control system can 
be configured to determine values of a plurality of velocity 
vectors representing time-derivative of a plurality of vectors 
indicative of positions of the base and a configuration of the 
ann and generating the control signals based on the velocity 
vectors. The control system can determine those velocity 
vector values by optimizing a merit function, which may 
incorporate one or more constraints associated with motion of 
the base and the ann. 

In yet another aspect, a robotic system is disclosed which 
includes a plurality of movable elements coupled to one 
anotherviaoneormorejoints defining one or more degrees of 
freedom of movement for each of the movable elements, at 
least one of said movable elements comprising a mobile base. 
The system can further include a user input device for receiv- 
ing input signals indicating a desired motion for at least one 
point on one of the movable elements (“end effector ele- 
ment”), and one or more processors communicatively 
coupled to the input device and generating, based on the input 
signals, control vector data representing position changes for 
the one or more joints and the mobile base to achieve the 
desired motion. One or more actuators can be coupled to the 
plurality of movable elements and to the one or more proces- 
sors, the one or more actuators effecting the desired motion by 
concurrently moving the mobile base and the at least one 
other movable element in accordance with the control vec- 
tors. 
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BRIEF DESCRIPTION OF THE DRAWINGS 

A further understanding of various aspects of the systems 
and methods discussed herein can be obtained by reference to 
the following detailed description in conjunction with the 
associated drawings, in which: 

FIG. 1 schematically depicts an exemplary robotic appa- 
ratus with a manipulator and a mobile base; 

FIG. 2 shows a graph of an exemplary joint-limit avoidance 
function for use in connection with optimizing a merit func- 
tion; 

FIG. 3 shows an exemplary illustration of a model for 
measuring the statistical error of a Euclidean space for use in 
connection with optimizing a merit function; 

FIG. 4 shows an exemplary representation of several ref- 
erence frames for a coordinated control model; 

FIG. 5 shows an exemplary representation of several links 
for a coordinated control model; 

FIG. 6 shows an exemplary representation of an aspect link 
in the model represented by FIG. 5; 

FIG. 7 shows an exemplary representation of an end effec- 
tor link in the model represented by FIG. 5; 

FIG. 8 shows an exemplary representation of relative links 
in the model represented by FIG. 5; 

FIG. 9 shows an exemplary block diagram of a control 
system in accordance with the teachings herein; 

FIG. 10 shows an exemplary block diagram of a software 
architecture for use with the control system of FIG. 9; 

FIG. 11 shows an exemplary flow diagram of steps to be 
executed by digital data processor(s) of the control system 
shown in FIG. 9; 

FIG. 12 is a schematic depiction of an exemplary two- 
wheel model for simulating a mobile base; 

FIG. 13 shows an exemplary dialog box illustrating the 
selection of end effector types and parameters; 

FIG. 14 shows an exemplary free-floating mechanism in 
simulation; 

FIGS. 15A-15D show a sequence of images of a exemplary 
model in free-base mode simulation; 

FIG. 16 shows an exemplary model for joint torque which 
accounts for base acceleration; 

FIG. 17 shows an exemplary illustration of an articulated 
body comprising a plurality of links; and, 

FIGS. 18A-18D show a side-by-side comparison of differ- 
ent algorithms used to simulate the movement of free-base 
mechanisms. 

DETAILED DESCRIPTION 

Certain exemplary embodiments will now be described to 
provide an overall understanding of the principles of the 
structure, function, manufacture, and use of the devices and 
methods disclosed herein. One or more examples of these 
embodiments are illustrated in the accompanying drawings. 
Those skilled in the art will understand that the devices and 
methods specifically described herein and illustrated in the 
accompanying drawings are non-limiting exemplary embodi- 
ments and do not limit the scope of the claims. 

A variety of exemplary embodiments will be disclosed 
herein. The features illustrated or described in connection 
with one exemplary embodiment may be combined with the 
features of other embodiments. Such modifications and varia- 
tions are intended to be included within the scope of the 
present application. 

In many cases, the methods and systems disclosed herein 
can be implemented using, amongst other things, software 
created as directed by the teachings herein and in accordance 
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with an object oriented programming scheme. Principles of 
object-oriented progra mmi ng and programming languages, 
(e.g., C++) are known in the art. Examples of software tool- 
kits which can be used to implement the systems and methods 
5 disclosed herein include the Actin and Selectin software tool- 
kits, developed by Energid Technologies, Cambridge, Mass., 
USA. Reference will be made to these software toolkits (e.g., 
to certain classes available therein) throughout this applica- 
tion for illustrative purposes, 
to 

A. Robotic Apparatus with Base 

Generally, disclosed herein are systems and methods for 
controlling apparatus, such as robotic apparatus or any other 
15 device requiring control. Such systems and methods include 
in particular those having robotic manipulators (e.g., an 
articulating arm with a tool such as gripper, grasper, saw, 
other tool, or a hand-like extremity) with a base. Although the 
base can be stationary, in many cases the base can be capable 
20 of moving (e.g., via wheels, tracks, or other mechanism). 
Many of the systems and methods disclosed herein can coor- 
dinate the control and corresponding movement of a manipu- 
lator and a mobile base. That coordinated control can be 
simultaneously effected across both the manipulator(s) and 
25 the base, e.g., both the manipulator and the base can be 
simultaneously or concurrently moved to effect to a desired 
motion, such a user-specified motion for an end effector, of a 
particular part of the robotic apparatus. 

FIG. 1 schematically depicts a robotic apparatus 100 made 
30 up of several movable elements 102, 104, 106, 108, 114. In 
this embodiment, the elements are coupled to one another via 
joints 116, 118, 120, 122 so as to create an articulating arm or 
manipulator 110 with a base 114. The base is mobile, as 
evidenced in FIG. 1 by several wheels 126 coupled to its 
35 sides. Virtually any other structure for moving the base could 
be used, such as tracks, treads, rails, cogs, propellers, jets, etc. 

In this case, each of the elements 102, 104, 106, 108, 114 
has one or more degrees of freedom in which it can move. 
Some elements may be able to move up and down (e.g., in a 
40 z-direction), side to side (e.g., in a y-direction), or extend 
distally (e.g., in an x-direction) or rotate, based for example 
on the capabilities of the joints coupling the elements to one 
another. Certain elements may be constrained in their move- 
ment, for example the base 114 can be on tracks limiting its 
45 sideways movement. Although any element could be so 
equipped, here element 102 includes a tool 112. In many 
cases such a tool 1 1 2 , or the element 1 02 , will be treated as an 
end effector, for which a particular motion is desired, however 
for controlling motion any element, including the base, can be 
50 treated as an end effector. 

B. Control Model and Control Strategy 

In one embodiment, a robotic apparatus, such as that shown 
55 in FIG. 1, can be modeled using a framework based on the 
manipulator Jacobian equation: 

V=J(q)q ( 1 ) 

where V is an m-length vector representation of the motion of 
60 one or more end effectors, which can be any movable 
element(s) (usually some combination of linear and angular 
velocity referenced to one or more points rigidly attached to 
parts of the robotic manipulator or the base); q is the n-length 
vector of joint and base positions (with q being its time 
65 derivative); and J is the mxn manipulator Jacobian, a function 
of q. (For spatial amis with a single end effector, V is often the 
frame velocity with three linear and three angular compo- 
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nents. It can also take on a larger meaning that includes the 
concatenation of point, frame, or other motion of multiple 
end-effectors.) When the manipulator is kinematically redun- 
dant, the dimension of V is less than the dimension of q 
(m<n), and equation (1) is underconstrained when V is speci- 5 
lied. By using V to represent relative motion, equation ( 1 ) can 
support self-connecting mechanisms by setting the relative 
motion to zero. 

As shown in FIG. 1 , element 102 can be designated as an 
end effector having at least one point with desired motion V. 10 
The manipulator Jacobian specifies the rate of motion of 
the robot parts doing work as a function of the rates of the 
degrees of freedom (joints and base motion). By using rates 
rather than positions, a difficult nonlinear system description 
is converted into a tractable linear equation for control. 15 
The velocity control question is the following: given a 
desired motion V, what are the rates q that best achieve this 
motion? To answer this, the framework uses a scalar a, a 
matrix function W(q), and a scalar function ;f(q) to solve for 
q, given V, through the following formula: 20 


J - 

‘1 v 

( 2 ) 

U/wJ 

-aNjFf 



where F=V;f is the gradient of f and N y is an nx(n-m) set of 
vectors that spans the null space of J. That is, JN y =0, and N, 
has rank (n-m). Both Vf and N y are generally functions of q. 
By changing the values of a, W, and f, many velocity-control 
techniques can be implemented. 

Equation (2) optimizes the following function: 


1 T T (3) 

j “=2 t/Wq + fF. 35 

Because q represents joint positions and base positions, 
this control approach coordinates arm and base motion — it 
works with wheeled, tracked, and walking mobile robots, as 40 
well as snake robots. 

Equation (3) can represent a merit function whose meaning 
depends on the selected values of a, W, and j 1 . For example, a, 

W, and f can be selected such that equation (3) represents, and 
equation (2) thus optimizes, kinetic energy use, avoidance of 45 
kinematic singularities, and so on. Hence, a desired control 
strategy (e.g., a user desired control strategy) can be imple- 
mented via selection of a, W, and f. 

Exemplary Control Strategies 

A variety of control strategies are possible. The control 50 
system is flexible and able to implement a wide variety of 
algorithms. For example, control types can include, without 
limitation, singularity avoidance, torque minimization, 
obstacle avoidance, fault tolerance, minimum-kinetic -energy 
control, minimum-potential-energy control, accuracy optimi- 55 
zation, and joint-limit avoidance 

Singularity Avoidance 

A finite-differencing tool was developed for implementing 
a control strategy that includes singularity avoidance. This 
tool takes a pointer to function object, and uses this function 60 
to numerically calculate its gradient with respect to joint 
variables using the finite difference method. This is a very 
flexible and powerful, though somewhat costly approach. 
Generally, it is better to explicitly calculate the gradient 
whenever possible. Since the explicit calculation is not 65 
always possible, finite differencing can be used for singular- 
ity avoidance. 
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A manipulator experiences a kinematic singularity when- 
ever the Jacobian loses rank. To frame the desire to avoid 
singularities into a solution method, a function is needed that 
is large at singularities and small away from singularities. For 
this, in some embodiments, the damped inverse of the product 
of the singular values of a weighted Jacobian is used. That is, 
the optimization function is the following: 


j w - ; — ■ 

<r\(T2 ■ ■ ■ cr„ + £ 

where € is a damping factor and a t is singular value i of the 
weighted Jacobian J, r : 

J^DjJDj, (5) 

where D r and D^are diagonal matrices. The weighting matri- 
ces can define the singular values of the Jacobian. 

In many cases, the singular values are not explicitly calcu- 
lated, as this is too costly. Instead the Cholesky decomposi- 
tion of J„J w r is taken, and the product of the diagonal terms 
is used. The product of these diagonal terms equals the prod- 
uct of the singular values. 

In some embodiments, the gradient of the function given in 
equation (4) is numerically calculated for use with the basic 
core through the class EcControlExpressionSingular- 
ity Avoidance, which is part of the aforementioned Actin tool- 
kit. For use with the AB core, it can be combined with EcCo- 
ntrolExpressionVectorToAB. These can be used with a 
positive definite weighting matrix and a positive scalar to 
drive the manipulator to minimize the function and therefore 
move away from kinematic singularities. 

Torque Minimization 

For torque minimization, the function to be optimized can 
be defined as the following: 

/(?)a 2 (?), ( 6 ) 

the square of the gravitational torque or force on joint i. To 
calculate the gradient, in this exemplary embodiment an 
explicit gradient calculation method (i.e., finite differencing 
is not used for this) for gravitational joint torque/force based 
on composite rigid body inertia is used. Using this calculation 
method, the gradient of the torque squared is given by: 

Vf(?)=2 & .(?)v a < ? ). (7) 

This is implemented in class EcControlExpressionGravi- 
tationalTorqueGradient for use with the basic core. For use 
with the AB core, it can be combined with EcControlExpres- 
sionVectorToAB. Using this value as the vector input to the 
core control system with a positive definite weighting matrix 
and a positive scalar produces control that minimized the 
magnitude of the gravitational torque on joint i. This can be 
used to prevent stress on a particular joint, or it can be used for 
failure recovery after a free-swinging failure. For more infor- 
mation, see “Fault Tolerance for Kinematically Redundant 
Manipulators: Anticipating Free-Swinging Joint Failures,” J. 
D. English and A. A. Maciejewski, IEEE Transactions on 
Robotics and Automation, vol. 14, pp. 566-575, 1998, which 
is hereby incorporated by reference. 

Minimum Kinetic Energy Control 

To minimize kinetic energy, the matrix parameter to the 
control system is set to be the manipulator mass matrix, and 
the scalar parameter is set to zero. That is, 


W=M(q), 

(8) 

a=0. 

(9) 
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In this case, when a=0, the vector parameter is not relevant. 
There are two classes for implementing this control strategy. 
For use with the basic core, EcControlExpressionMassMa- 
trix returns the manipulator mass matrix — M(q) in equation 
(8) — directly for use as weighting parameter W. The manipu- 
lator mass matrix is calculated using the method of Compos- 
ite Rigid-Body Inertia (developed in “Efficient Dynamic 
Computer Simulation of Robotic Mechanisms,” M. W. 
Walker and D. E. Orin, Journal of Dynamic Systems, Mea- 
surement, and Control, vol. 104, September 1982, pp. 205- 
211 and named in Robot Dynamics Algorithms, R. Feather- 
stone, Kluwer Academic Publishers, 1 987, both of which are 
hereby incorporated by reference), which is extended to apply 
to bifurcating arms. The same algorithm is used to calculate 
the mass matrix for minimum-kinetic-energy control as is 
used to calculate the composite-rigid-body dynamics. For use 
with the AB core, an A parameter corresponding to the 
manipulator mass matrix as W is calculated by treating the 
Jacobian-null-space basis vectors as accelerations and calcu- 
lating the corresponding joint torques. In this exemplary 
embodiment, for this calculation the iterative Newton Euler 
algorithm is used; for more information see “On-Line Com- 
putational Scheme for Mechanical Manipulators,” J. Y. S. 
Luh, M. W. Walker, and R. P. C. Paul, Journal of Dynamic 
Systems, Measurement, and Control, vol. 102, pp. 69-76, 
June 1980, which is hereby incorporated by reference. 

Minimum Potential Energy Control 

To minimize the potential energy of the arm, the gradient of 
the potential energy is used. For articulated mechanisms, the 
gradient of potential energy is composed of gravitational joint 
torques. To calculate the gravitational joint torques (forces for 
prismatic joints) the composite rigid-body inertia of the links 
is used. This method is implemented for the basic core in the 
class EcControlExpressionPotentialEnergyGradient. 

Accuracy Optimization 

For accuracy optimization in the presence of position 
errors in a single joint, an F value is provided for the basic core 
through the class EcControlExpressionError Sensitivity. For 
this calculation, a weight w, is assigned to each of E end- 
effectors on the manipulator, andafocus joint, j, is identified. 
Then an error sensitivity function is defined as follows: 


E 

/(?) = ^W,lljV,|| 2 , 


(10) 


where^v, is the velocity of end-effector i due to unit motion of 
joint j. 

The gradient of this Junction is then found using finite 
differencing, and this is used as the vector parameter for the 
core velocity control system. With the matrix parameter being 
a diagonal matrix and the scalar parameter a positive value, 
this serves to minimi ze the sensitivity to errors. 

Joint-Limit Avoidance 

For joint-limit avoidance, for each joint, a rate term is 
independently calculated that is a polynomial function of the 
proximity to a limit. This is illustrated in FIG. 2, which shows 
how the vector elements for joint-limit avoidance are set 
using polynomials with a user-configurable exponent and 
dead-zone. Class EcControlExpressionJointLimitAvoidance 
implements this weighting function. This is used with a nega- 
tive scalar value to drive the manipulator away from joint 
limits. 


Strength Optimization 

Strength optimization is achieved by employing a tech- 
nique based on the strength formulation in “The Minimum 
Form of Strength in Serial, Parallel and Bifurcating Manipu- 
lators,” R. O. Ambrose and M. A. Diftler, Proceeding of the 
1998 IEEE International Conference on Robotics and Auto- 
mation, Leuven, Belgium, May 1998, which is hereby incor- 
porated by reference. In this discussion, torque is used in the 
10 general sense, meaning force for a sliding joint. For an arbi- 
trary set of end-effector forces, a vector of joint torques can be 
found. The goal of strength optimization is to adjust the 
manipulator joint positions in such a way as to minimize the 
normalized joint torques resulting from the imposed forces 
15 while simultaneously placing the hand(s). This is accom- 
plished by directing the joints toward the direction that yields 
the maximum reactive strength in the direction opposing the 
applied forces . Strength here is defined as the maximum force 
or moment that the manipulator can exert on its environment 
20 at the point of resolution of an end effector. 

The basic formulation of velocity control used to relate 
joint rates to hand velocity is given by equation (1). Again, V 
is a concatenation of hand velocities, q is the vector of joint 
-, 5 rates and J(q) is the manipulator Jacobian. 

If friction and gravitational loads are neglected, the power 
flow at the joints must equal power flow at the end effector. By 
relating hand velocities V and corresponding wrench loads F 
with joint rates q and joint torques T, this property is 
30 expressed as follows: 

V r F=-q r T (11) 

Using equation (1 ) with the fact that equation (11) must be 
35 true for all values of q yields 

T=-J(q) r F (12) 

Equation (12) relates wrench loads exerted on the system 
with j oint torques required to prevent manipulator motion due 
to those loads. 

When assessing the total strength of a manipulator, torque 
capacities for each joint must be known. The torque capacity 
is typically limited by the motor’s ability to supply torque or 
45 the gearhead’s ability to transmit torque to the joint. The 
torque capacity T for a given joint can be expressed as 

f=min(r a G,y (13) 

where t a is the actuator (corresponding to motor) torque, G is 
the gear ratio (the ratio of actuator velocity to joint velocity — 
typically G is greater than unity) and t t is the transmission 
torque. The torque capacity for a joint may differ depending 
on direction. 

55 To optimize manipulator configuration for strength, let x be a 
desired end-effector force, as would be used with F=x in 
equation (12). Let T max . be the maximum torque capacity, and 
T min be the minimum torque capacity (the largest magnitude 
in the negative direction) for joint j. For an n-joint system, a 
60 measure of strength f(q) is given by 


65 


/(<?) = 


nr - 1 

z 




( 14 ) 
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where is the actual torque required to achieve F=x using eq. 
(12), (.^ is the mean of the torque capacity of joint j, given by: 


(15) 5 


t*r- 


and ||T|| is the range of torque capacity for joint j, given by: 

\\f,\\=T max -f mi „.. (16) 

u j ii maxj mm } \ / 

The user-defined parameter r specifies the order of the 
function to optimize. Note that r=2 corresponds to a sum of 
squares approach and r— *oo approaches an infinity-norm 
approach, where the strength is defined only by the weakest 
joint. 

The gradient of f(q) is found using finite differencing, 
forcing the joints in the direction of minimum normalized 
torque and, hence, maximum strength. This gradient is cal- 
culated in the expression tree using the class EcControlEx- 
pressionStrengthOptimization. Because it is a gradient, it is 
for direct use with the basic core velocity control system. It 
can be converted to a B parameter for use with an AB core by 
combining it with an EcControlExpressionVectorToAB 
expression. 

Statistical Error Reduction 

For accuracy optimization in the presence of high fre- 
quency noise on multiple joints, an approach is used that 
exploits the statistical properties of the joint noise as well as 
the Euclidean Space nature of the tools the manipulator is 
using. 

The Function for Optimization 

Let the velocity error in the manipulator’s joints, q^, be a 
random variable having covariance C e . That is, the expected 
value of the quadratic form is given by 


m.-q. T ]=c e 


(17) 


It is assumed that the controller prevents low-frequency 
drift to enforce E(q e )=0. It is desired to minimize the expected 
value of a quadratic measure of the hand velocities due to this 
error. Let the error in the hand velocities be W e , now also a 
random variable. From equation (1 ), the relationship between 
q e and V e is 

(IS) 

Let a measure of the hand error be defined as 

ii.=r/AV e , (19) 


x r MX=tr(Mxx r ). 

This gives 

J \q)=E[tr(J r AJqJi/)\. 


f(q)=tr(J r AJC e ), 

a straightforward function of configuration. 
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for some prescribed constant matrix A. Note p c is a random 
variable. The goal, then, is to minimize the expected value of 
\i e . That is, the function to be minimized is given by 

m=E[M e ] ( 20 ) 

Combining the three above equations gives 

Kq)=E[q e T J T AJq e }. (21) 

To put this in a form that can be used for optimization, the 
following formula is used, which applies for any column 
vector x and any matrix M, 


50 


( 22 ) 


(23) 


Here, tr(*) is the sum of the diagonal entries (the trace 
operator). Using E(q f ,)=0, this gives 


( 24 ) 


Evaluating the A Matrix 

The matrix A as used above can be set in a number of ways . 
By way of example, one approach is to use Euclidean-Space 
regions of interest rigidly attached to the hands . Such a region 
might be the physical extent of a tool, for example. 

Let the Euclidean-space region be labeled Q. Then p e can 
be cast to minimize an integral of the velocity error squared 

over Q. Let r be a point in Q, and let p( r ) be a nonnegative 
interest density function defined over Q (specifying, for 
example, that the tip of a screwdriver is more important than 

the shaft). Let v c ( r ) be defined as the error velocity at point 

7 , a random variable. These are illustrated in FIG. 3, which 
shows the terms used to measure the statistical error of a 
Euclidean Space region in the region rigidly attached to one 
end-effector frame. 

With this, what is desired is to minimize 


/(<?) 


= £[ £p(r)l(v e (r 


(?)lf<M 


(25) 


That is, the expected value of a weighted integral of the 
velocity error squared of all the points in the region of interest. 
This is a comprehensive and intuitive measure. 

It is not practical, in many cases, to calculate the integral 
shown in eq. (25) in real time. However, there is a way to cast 
it into the form of eq. ( 1 9), which will allow eq. (24) to be used 
with no on-line integration. To do this, an analogy is used 

between eq. (25) and kinetic energy of a rigid body. If v e ( r ) 

were the velocity over the region Q and p( r ) were the mass, 
then the integral in eq. (25) would provide twice the kinetic 
energy of the body. 

Let the following be defined in analogy to mass, first 
moment of inertia, and second moment of inertia: 


ntn = p(r )dco, 
J n 

hn = ^ p(r)rda>, 


In = 


£ pi ')« 7 


Rdco , 


(26) 

(27) 

(28) 


where R is the cross product matrix for r (i.e., Rx= r xx for 
any column vector x). 

With these values, let the following matrix be defined: 


7n - 


m n I 

Hi 


Hn 

In 


(29) 


where I is the 3x3 identity matrix and H Q is the cross-product 
matrix for h Q . This produces, for a single frame end effector, 
the following: 


dbJ = V ' J n W 


( 30 ) 
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Equation (30) the same form as eq. (19), allowing the 
optimization of eq. (24) to be performed using eq. (24) with 

a=j q . 

In general, there will be N end effectors, and the matrix A 
will be block diagonal: 5 


[Au 


[Awl 


[Ai,a 


(31) 


10 


Implementation of Statistical Error Reduction 
Statistical error reduction is implemented through the 15 
EcControlExpressionError Reduction class. This gives a vec- 
tor weight for use with the basic core velocity control system. 

It can be converted to a B parameter for use with an AB core 
by combining it with an EcControlExpressionVectorToAB 
expression. 20 

End-Effector Descriptions 

The array V, as used above, represents the motion of all the 
manipulator’s end effectors. A special class can hold the 
description of an end-effector set, which contains any number 
of end effectors. The end effectors can represent any type of 25 
constraint. Implemented end-effector types include frame, 

3D point, 3D orientation, 2D point, center of mass, and spatial 
momentup. More types can be added using the toolkit or the 
plugin interface. 

To allow this general approach, many of calculations 30 
needed for velocity control are performed in the end-effector 
class. The public methods that must be implemented to define 
a new end effector are given in the table below. 


tions allow the end-effectors to create their own Jacobians and 
position controllers. Subclasses of EcEndEffector include 
EcFrameEndEffector, EcPointEndEffector, and EcXyEndEf- 
fector. 

Any of the foregoing control strategies, and others, can be 
implemented using a dynamically updatable control system 
of the type described inU.S. Pat. No. 6,757,587 of English et 
al. (“Method and Apparatus For Dynamically Reprogram- 
ming Remote Autonomous Agents”), which is hereby incor- 
porated by reference in its entirety. For example, mission 
control can select expressions or values for one of more of a, 
W, and f components of the control system equations, 
expressing those components as an XML file and communi- 
cating it to a control system (e.g., a robot). The robot can 
validate the XML file using an on-robot copy of the schema, 
and use the validated XML to create in memory, at run time 
and using a recursion and a creator function, a code tree 
structure representation of the components of which the XML 
file provides an expression or a value. 

C. Derivation of Control Model 

The exemplary control model disclosed above, including 
coordinating control of mobile or floating bases concurrently 
with an arm, will now be described in more detail. 

The use of mobile manipulators can be modeled using a 
reference-frame hierarchy. In the framework, the location of 
the base of each manipulator can be specified through a 
sequence of transformations, as illustrated in FIG. 4. The 
entire manipulator system is represented in the system frame, 
which is defined with respect to a universal reference frame as 
part of the environmental specification. Then, each manipu- 
lator has a static base frame whose location with respect to the 


TABLE 


Member functions that are implemented to define a new type of end effector. 

Member Function 

Meaning 

doc 

Returns the end effector’s degrees of constraint. For a point 
end effector, it returns 3. For a frame end effector, 6. 

insertJacobianComponent Builds a strip of the Jacobian. The height of the strip equals 
the value returned by doc. 

insertSparsityComponent 

Builds a strip of the sparsity description of the Jacobian. A 
value of true in this strip means the corresponding position in 
the Jacobian is always zero. The height of the strip equals the 
value returned by doc. 

calculatePlacement 

Calculates a placement value for the end effector. The 
placement is described through an 
EcCoordinateSystemTransformation, which may have 
different meanings for different end-effector types. 

calculate Velocity 

Calculates end-effector velocity. The result is a real vector of 
length equal to the value returned by doc. The velocity will 
have different meanings for different end-effector types. 

calculateAcceleration 

Calculates end-effector acceleration. The result is a real 
vector of length equal to the value returned by doc. The 
acceleration will have different meanings for different end- 
effector types. 

filterEndEffectorVelocity 

Calculates an end-effector velocity that drives the end effector 
toward a guide placement. The guide frame is always 
represented in system coordinates. 

minimumTime 

Calculates the minimum time that will be used to move from 
one frame to another. 

difference 

Calculates a measure of the difference between two 
placement frames that uses Euclidean distance as its baseline. 
That is, the difference between two frames is the Euclidean 
distance between them plus optional additional factors related 
to orientation change. 


Through this approach, any new end effector can be added 65 system frame is specified as part of the manipulator descrip- 
as a subclass of EcEndEffector, provided these member func- tion. The (dynamic) location of the manipulator with respect 

tions are implemented in the new class. These member func- to the static base frame is specified as part of the state, through 
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the dynamic base frame. FIG. 4 illustrates the location of a 
manipulator’s base frame. The dynamic base frame location 
is identified through a sequence of transformations. For 
mobile manipulators, the motion of the dynamic base frame is 
integrated with the motion of the joints. 

The dynamic base frame specifies the location of the base 
link on the manipulator, as illustrated in FIG. 5. FIG. 5 illus- 
trates how one link on the manipulator serves a special role as 
the base link. It does not have a joint associated with it. 
Instead, the location of the dynamic base frame determines its 
location. In an exemplary software implementation, in the 
code the base link is a subclass of the normal link type that 
stores information on the entire manipulator. 

Aspect Link 

In addition to the base link, there is another link that can be 
used as a reference for controlling the manipulator when the 
base is not fixed. This is the aspect link, as shown in FIG. 6. As 
opposed to the base link, the aspect link is conceptual (i.e., 
any link can be assigned as the aspect link). As an object in 
code, it is just another link. FIG. 6 illustrates how one link on 
the manipulator serves a special conceptual role as the aspect 
link. It is a regular link, distinguished as the aspect link only 
through the control system. Fora free-floating base, the frame 
velocity V a is used for calculating kinematic control. 

Moving-Aspect Jacobian 

Each end effector is defined as rigidly attached to some 
link. This end-effector link can be any link of the manipulator, 
and is typically separate from both the base link and the aspect 
link. This is shown in FIG. 7, which depicts how the end- 
effector link, can be rigidly attached to the end effector for 
which kinematic control is desired. 

For end-effector position r e and base position r b , let 


P b—e = r e - r b . 

And, for any vector p , let P be defined as: 


(32) 


P = 


I 

0 



(33) 


This allows the moving-aspect Jacobian to be defined as 
J* e a =[J e -P^J a \P a ^ e \ (40) 

Let q* a be defined by concatenating q and V a as follows: 


to 



Then 


V e — J e #2 


(41) 


(42) 


15 gives the manipulator Jacobian equations when the aspect 
link is moving. 

Using this with 


i*a 

J e 

-i 

V, 



-aNj.cF; 


where, forN links, W* a is an (N+6)x(N+6) symmetric matrix 
and F* is an (N+6)xl column vector, gives a value of q* that 
produces the desired V e while minimizing 


1.7-., , T , (44) 

30 z 

This is a desirable result. An arbitrary quadratic function of 
q and \’ a can be minimized this way. However, to calculate 
equation (43) directly involves calculating a special Jacobian 
35 as a function of the attribute frame, which can be disadvan- 
tageous in some cases. 

To avoid this, there is an alternative representation that 
allows the regular manipulator Jacobian to be used, yet 
achieve the same result. More specifically, from equation (36) 
40 the following equation holds: 


with 


r s 


I px 
0 / 


(34) 


45 


With this definition, the frame velocity of the end effector 
is given by: 


v b =P^ b V a -P a ^ bJa q, 
Let T a _ b be defined as 


L >b = 


and 


-Pa^bJa Pa^b i 


(45) 


(46) 


V=Jj+P^.r b , (35) 50 

where J e is the manipulator Jacobian. All quantities here are 
assumed to be represented in the same frame. 

Similarly, for the aspect link the frame velocity of the 
aspect frame is generally: ^ 

V=J„q+P^V b , (36) 

where J a is the manipulator Jacobian for the aspect frame. The 
matrix can never be singular, allowing: 

Combining (45) and (47) gives 

r=JA+P b ^J’ b ^-\v a -JJD, 

Using Pb^ePb^a~ 1= Pa^e an d collecting terms gives 


(37) 


(38) 



This gives 

Let J* e be defined as 
This can then be used with 


K 

-1 

V e 

_ Nj t <j£bfWST£b_ 


-a Nj,(T^\ b ) r F- 


(47) 


(48) 

(49) 


(50) 


r=(j,-p^Mq+p a ^,r^ 


(39) 


to give the same result as (43). 



Let 
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TABLE 


W*=(T a _ b l yw\T a _ b l 

(51) 

Method b\ 

r which the various come 

fonents are specified. 

and 

5 

System 

Velocity 

Control 

End Effector 

Note has the following convenient form: 

(52) 

10 - 

Base Link 
Base Mobility 

Jacobian 
Aspect Link 
Aspect 
Mobility 

End Effector Link 
Relative Link 


.!_[■/ 0 1 ( 53 ) 


With this, 


r l-‘r v (54) 

. * _ J e v e 

qb ~ Nj, W -aNj. F“ 

J e J L J e 

gives the same manipulator motion as (43), but without the 
need to calculate J*/. The choice of aspect frame is incorpo- 
rated only into the parameterization values W* and F* (note 
the structure of (54) is the same as that of (43)). 

Relative Jacobians 

An end effector can be defined relative to another frame on 
the same manipulator. In this case, the Jacobian equation is 
the same whether or not the aspect link is mobile. If a relative 
end effector is defined, a new special link is added to the 
manipulator, as shown in FIG. 8. FIG. 8 illustrates how an end 
effector can be defined relative to any other link. 

Let s V e be the frame velocity of the end-effector with 
respect to and expressed in the system frame, with linear 
velocity s v e and angular velocity s u> e . The point of application 

for these values is r e . Similarly, Let s V r be the frame velocity 
of the reference frame with respect to and expressed in the 
system frame, with linear velocity s v r and angular velocity 

s a> r . The point of application for these values is 7,.. 

With these definitions, Jacobians J e and J,. exist such that 

s V=Jj+P^, s V b . (55) 

and 

s V=JA+P b ^ r s V b . (56) 

Then the velocity of the end effector with respect to the 
relative frame is 

r V.= S V„-P e ^ r ss r r . (57) 

The effect of V 4 cancels, leaving the following Jacobian: 

Jl=J r -Pe-,r Sj r- ( 58 ) 

This has no dependence on the motion of the base. 

Specification of Mobile-Base Parameters 

The description of the base, aspect, relative, and end-effec- 
tor links should be given in the control-system description. 
Whether or not the aspect link is moving or stationary should 
be specified. Possible methods for specifying these are 
through the manipulator system, the manipulator state, the 
velocity control system, or the end-effectors. 

In some implementations, the mobile-base parameters are 
specified as follows: 


D. Exemplary Hardware/Software Implementations 

FIG. 9 depicts an exemplary block diagram of a control 
system 900 in which input signals 912 come from, e.g., user 
input device 902. A user input device can include, e.g., any 
input device known in the art associated with a computer, 
such as a keyboard or mouse, screens (e.g., touchscreens), 
joysticks, steering wheels, and other input devices which may 
be tailored to the apparatus they are controlling. The input 
signals 912 generated by the user input device 902 can be 
used to input and/or indicate the desired control strategy for 
the control system (e.g., as represented by parameters a, W, 
and j\ described above) and/or the desired motion (e.g., 
velocity vectors) of the apparatus (e.g., by manipulation of a 
joystick, for example). Input signals 912 can also be gener- 
ated by sensors 904, such as machine vision, contact sensors, 
position sensors (e.g., GPS), gyroscopes, and so on. 

In the exemplary embodiment of FIG. 9, input signals 912 
are fed into digital data processor(s) 906, which can be 
embodied in any general or special purpose computer. In 
many cases the user input device 902 and user display 918 can 
be integrated with the digital data processor 906 in a manner 
known in the art to create a familiar personal computer, lap- 
top, PDA, and so on. The digital data processor 906 can take 
the form of an operator control unit (OCU) with a user inter- 
face (e.g., a software interface providing a GUI or text -based 
interface) in accordance with the teachings herein. In some 
embodiments, the digital data processor 906 can be commu- 
nicatively coupled, e.g., via a network, wirelessly, or other- 
wise, to the apparatus (e.g., apparatus 920) which it controls 
(e.g., for communicating control signals 914). In other 
embodiments, the digital data processor 906 can be embed- 
ded, e.g., within the controlled apparatus. 

The digital data processor 906 includes algorithms stored 
on a computer-readable medium, e.g., magnetic, optical, 
solid-state or other storage medium. Such algorithms can 
effect control in accordance with the control principles dis- 
closed herein and may include standard off-the-shelf soft- 
ware packages for implementing certain functionality. Such 
software packages may include, for example, the Actin (for 
effecting control) and Selectin (for machine vision) software 
toolkits developed by Energid Technologies of Cambridge, 
Mass., USA. One or more processors of any type known in the 
art can execute the instructions embodied in algorithmic form 
in view of the input signals received from the user input 
devices and/or sensor devices to effect control of the appara- 
tus. 

Control signals 914 generated by the digital data processor 
can be communicated to the controlled apparatus 920 and 
more particularly, in many cases, to one or more actuators 916 
in such apparatus. The controlled apparatus 920 can have its 
own local intelligence, e.g., processing capability, for inter- 
preting the control signals 914, translating the control signals 
914 to a form appropriate for the actuators 916, and/or, in 
some cases, determining some or all of the control signals 914 
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itself. Actuators 916 may take the form of any device capable 
of being controlled or instructed, including motors, servos, 
pneumatic devices, magnetic devices, electromagnets, and so 
on. In some cases, the control signals 914 may be transformed 
to the appropriately (e.g., to an appropriate voltage, current, 
or communications protocol) for the actuators 914 to which 
they are assigned. Regardless, the actuators operate under the 
direction of the control signals 914 to effect the desired posi- 
tion changes and/or motion of each movable element of the 
controlled apparatus 920. 

FIG. 10 depicts another block diagram of an exemplary 
top-level software architecture 1000 for controlling an appa- 
ratus such as a robotic apparatus and references exemplary 
software packages, such as the above-referenced Actin and 
Selectin software packages. As shown in FIG. 10, informa- 
tion is exchanged A) between the operator control unit (OCU) 
and the coordinator on the robotic apparatus, B) between the 
coordinator and the robot Selectin-based vision module, C) 
between the coordinator and the Actin-based force and posi- 
tion control, D) between the coordinator and the grasping 
algorithm database, E) camera imagery from the robot plat- 
form, F) control interface for the robot platform (in some 
cases, a software interface provided by a robot manufacturer, 
e.g., Aware 2.0 for iRobot’s Packbot), and G) between the 
OCU and the Selectin-based vision module. The robot and 
interface, marked with a box on the left, can be optionally real 
or simulated. 

FIG. 11 depicts an exemplary flow-chart of steps that can 
be executed in a digital data processing system, e.g., as shown 
in FIGS. 9-10, that functions in accordance with the teachings 
herein. As shown in FIG. 11, in step 1100 the system can 
receive data indicating the selected control strategy, e.g., as 
represented by a merit function to be optimized with param- 
eters a, W, and f as described above in connection with 
Equation (3). Further, the system can receive input signals 
indicating a desired motion at step 1102. Such input signals 
may take the form of desired velocity vector V for an end 
effector (e.g., as described above in connection with equa- 
tions ( 1 ) and ( 2 )), which can take the form of an m-length 
vector. In step 1104, the system can generate control signals 
(in some cases the control signals can represent control vec- 
tors representing a temporal rate of position changes for each 
joint, as described above in connection with q, for example). 
The control signals can be optimized in accordance with the 
merit function (such as equation (3), for example) and/or 
selected control strategy. In many embodiments, the control 
signals can be generated using equation (2). In step 1106, the 
control signals can be applied to a robotic apparatus being 
controlled, e.g., to the software interfaces provided for that 
apparatus, which can apply the proper signals to actuators to 
effect movement (e.g., the position changes mentioned 
above.) In other embodiments, the apparatus software inter- 
faces may be omitted or unnecessary and the control signals 
applied to the actuators directly. At 1 1 12, the apparatus exhib- 
its the desired and optimized motion. In many cases, such 
motion involves simultaneously moving a mobile base and 
other movable elements (e.g., making up a manipulator). 

E. Examples & Simulations 

By way of further illustration, the following examples are 
included to further elucidate, among other things, the prin- 
ciples described above. The examples include exemplary 
simulations provided via software toolkits developed in 
accordance with the teachings disclosed herein. It should be 
understood that the information presented in connection with 
these examples is provided for illustrative purposes and is not 
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intended to necessarily show optimal results that can be 
achieved or that need to be achieved by employing control 
systems and methods in accordance with the teachings of this 
application. 

5 

Examples 1 and 2 

Coordination Control For Mobile Base 

io Presented below are two simplified kinematic simula- 
tions — a one degree-of-freedom sliding end effector and a 
planar end effector. The term end effector used here refers to 
controlling the base as an end effector. In a kinematic simu- 
lation, any link, including the base, can be treated as an end 
15 effector. Many different kinds of end effectors are imple- 
mented in the Actin software toolkits. The naming of these 
end effectors usually reflects kinematic restrictions rather 
than functions. For example, a point end effector indicates 
that the three dimensional position of the end effector needs to 
20 match that position with the desired one (the origin of the 
guide frame in the GUI. The guide frame is a visualization of 
a set of three orthogonal axes that shows the desired position 
and orientation for the end effector; for a point end effector, 
the orientation does not matter. The frame end effector can 
25 match the guide frame in both position and orientation. In the 
GUI, the user manipulates the guide frame via mouse or other 
input devices and the selected end effector follows the guide 
frame in a different way based on the different end effector 
type. Note that there can be more than one end effector active 
30 at the same time. For example, if the base of the robot has been 
assigned to a frame end effector and its gripper has been 
assigned to another; then manipulating the gripper’s guide 
frame will move the gripper around while keeping the base 
fixed. 

35 This example illustrates coordinated motion between an 
ami and the base. In this case, the sliding end effector men- 
tioned above will be assigned to the base and it will allow the 
base to move back and forth, no turning or side moves. The 
guide frame for the base’s sliding end effector is not moving; 
40 we only move the gripper’s guide frame. The planar end 
effector works in a similar fashion. Instead of allowing the 
base moving back and forth; the base can move freely on the 
x-y plane (with z pointing up). Note that the tracks will not be 
able to move side ways, so any velocity in the side direction 
45 needs to be filtered out. The following sub-sections explain in 
detail how these end effectors and the filter are implemented. 

Example 1 

50 Implementation of Sliding End Effector for the Base 

In this example the mobile base is modeled after an end 
effector which exhibits sliding motion. In other words, con- 
straints are placed on the movement of a mobile base, e.g., to 
55 model those constraints that might exist with real bases. 

The relation between the end effector velocity and the joint 
velocity can usually be written as the Jacobian equation: 

V=J(q)q, ( 59 ) 

60 where V is an m-length vector representation of the motion of 
the end effectors; q is an n-length vector of joint and base 
positions (with q being its time derivative); and J is the mxn 
Jacobian matrix as function of q. This example will illustrate 
control of the mobile base as a single degree of freedom 
65 mechanism that can move forward and backward. One way to 
achieve this is to model the mobile base as a six degree of 
freedom object and apply a five degree of freedom constraint 
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on it. We define a frame attached to the base and assume the 
base can only move in the frame’s x direction; its orientation 
and y, z coordinates will not change. This part of Jacobian is 
written as 


exemplary two-wheeler model for this is shown in FIG. 12. 
FIG. 12 illustrates the model for mobile platform on a 2-di- 
mensional plan. Distance between the wheels is equal to w, 
wheel radius is equal to r. 

With reference to FIG. 12, while the frame attached to the 
axel has three degrees of freedom (x, y and 0„), the joints only 
have two degrees of freedom (0 O and 0 X ). The relation 
between the joint velocity and the frame velocity can be 
written as: 


where v e and w e are the linear and angular velocity of the end 
effector frame in the system coordinates; v b and v/ b are the 15 
linear and angular velocity of the base in the system coordi- 
nates. Note that J(q) is only part of the whole Jacobian matrix 
that corresponding to the constraint of this end effector. The 
relation between v e , w e and v b , w A can be written as 


V b + Wb x p, 
w b 


0 P, - Py 

-Pi o Px r Vf, 

Py ~P X 0 [W,, 


where p e =[p x p x p J T is the linear transformation from the base 
frame to the end effector frame represented in the base frame. 
Since the constraints are given in the end effector frame and 
the end effector velocities are calculated in the system coor- 
dinates, these vectors will be converted into the end effector’ s 
frame, as follows: 
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rcos0 z 
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Note that the Jacobian matrix relating the linear velocity x, 
y of the end effector and the joint angle velocities 0 O and 0 X is 
singular; meaning position error can not be corrected through 
end effector control if there is no angular error. 

One way to solve this problem is to correct the position 
error by rotating the platform to the direction of desired linear 
velocity, then move the platform forward or backward. Once 
the position error is minimized, the angular error can be 
corrected. This procedure can be implemented in our control 
software using an end effector velocity filter. Instead of using 
equation (63), we can follow the similar steps as in the pre- 
vious example. First, we form the Jacobian matrix for the end 
effector velocity represented in the end effector frame as in 
equation (62). That is, we treat the base as a 6 degree of 
freedom object in space. Then we let the base move freely in 
x, y and 0.. by dropping the first (x), second (y) and the sixth 
(0_) rows of the Jacobian matrix 


Therefore the portion of the resulting Jacobian matrix for this 
end effector is obtained by taking the last five rows of the 
matrix 


This new partial Jacobian matrix is of size 5x(k+6). 

The implementation of this class is in Actin-control EcS- 
lidingEndEffector. The robot (e.g., iRobot’s Packbot) base 
end effector frame has x pointing forward, z pointing up and 55 
the y pointing the side way. With this end effector defined to 
the base, the base can move forward and backward; which add 
one more degree of freedom to the system and makes the arm 
a 6 degree of freedom arm instead of 5. 

60 

Example 2 

Planar Motion Base Simulation 


which results in a Jacobian matrix of size 3x(k+6). The imple- 
mentation of this class is in actin-control EcPlanarEndEffec- 
tor. 

In this example, a constraint is placed on the base such that 
the tracks cannot move in y (side ways). To achieve this, the 
planar end effector can be combined with a velocity filter to 
restrict the mobile platform from sideways (y) motions. The 
resulting velocity of this filter is achievable with tracks (e.g., 
a tracked mobile robot). 

The planar end effector acts as a restriction which adds 6 
elements to the joint positions q in the Jacobian equation: 


and 3 velocity terms to V. For this portion of the Jacobian 
equation, it can be written as 


»'V = J(q)\ 9 k 


This example for the mobile base kinematic simulation 65 

illustrates planar motion; where the platform (mobile base) where z is the linear velocity represented in the end effector 

can move in the x-y plane and rotate about the z-axis. An frame (a frame attached to the mobile platform), w^andw are 
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the angular velocities of the end effector frame in the end 
effector coordinates; v A and w A are the linear and angular 
velocities of the DH frame of the mobile platform in the 
system coordinates. Note that there is a DH frame and a 
primary frame defined in each link. DH frames are defined for 5 
the kinematics and primary frames are the coordinates system 
defined for the CAD model of each link. There can be offset 
between the DH frame and the primary frame in each link. 
Also note that the end effector frame is defined with respect to 
the primary frame. Therefore when filtering the joint veloci- to 
ties for the base, v b and w A given in the DH frame, we need to 
convert them to velocity of the end effector frame represented 
in the end effector frame. Exemplary pseudo code is listed as 
below: 


EEToDHInSystem = baseEEFrameInSystem.translation( ) - 
baseDHFrameInSystem.translation( ); 

EEVelocitylnSystem = 

DHVelocitylnSystem.transformBy(EEToDHInSystem); 20 

EEVelocitylnEE = 

EEVelocityInSystem.transfomiBy(baseEEFrameInSystem.orientatioii( ).- 
inverse( )); 

filteredEEVellnEE = EEVellnEE; 

// only thing we did here is setting the side velocity y to 0 
filteredEEVellnEE.setLinearY (0.0); 25 

// convert velocity back in system coordinates. 
filteredEEVellnSystem = 

filteredEEVelInEE.transformBy(baseEEFrameInSystem.orientation( )); 

// calculate the corresponding velocity at the DH frame 
filteredDHVellnSystem = 

filteredEEVelInSystem.transformBy(-EEToDHInSystem); 


In this example, the side movement velocity can be set to 0. 
Depending on the desired behavior of the tracks, different 
behaviors can be introduced as needed by adding new types of 
velocity filters. Special attention can be paid to the different 35 
frames (primary, DH and end effector frames). Denavit- 
Hartenberg (DH) parameters specify the reference frame of 
any link in a robotic system with respect to an adjacent link. 

In this way, for a connected chain of links, the position and 
orientation of each link is specified relative to the position and 40 
orientation (reference frame) of an adjacent link. 

One exemplary filter is a joint rate filter implemented in the 
class in Actin-control EcControlExpressionJointRateFilter. 
The planar tracks filter and the joint rate filter are applied 
sequentially. The end effector and the end effector frame 45 
defined with respect to the primary frame can be changed on 
the fly through the GUI. The end effector dialog allows the 
user to select a manipulator in the system and the desired link 
in the manipulator. In this case (see FIG. 13) we want to 
assign a planar end effector to the base. A pull down menu, 50 
illustrated in FIG. 13 shows a dialog box for assigning end 
effectors and setting up parameters, which has a variety of 
implemented end effectors. For each type of end effector 
there is a list of associated parameters. The user can change 
the parameter values through the dialog box. 55 

A demo is created with the newly implemented sliding end 
effector and the planar end effector combined with the planar 
tracks filter to demonstrate the coordinated arm and mobile 
platform motion. The arm or manipulator (e.g., an explosive 
ordinance disposal arm or EOD) is typically operated with the 60 
robot base fixed. The new control scheme provides a way to 
operate them together. This can have the advantage of giving 
the operator a single interface that is more intuitive. In addi- 
tion, the integrated system has more degrees of freedom than 
operating the base and ami separately. This higher degree of 65 
freedom provides many benefits when dealing with collision 
avoidance, joint limit avoidance and many other secondary 
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goals. In the simulation we assigned a point end effector 
(restricts just the location but not the orientation) to the cam- 
era and a planar end effector to the base. We moved the 
desired camera end effector position by dragging a guide 
frame in the viewer and the camera follows. In the simulation 
it can be seen that: 

1 . The integrated system lias more degrees of freedom, has 
a larger workspace, and can be controlled through a 
single user interface. 

2. The planar end effector and the planar tracks filter effec- 
tively constraint the mobile base to behave like a tracked 
mobile platform; it can move back and forth and can 
spin, but cannot move sideways. 

3. We defined a rough bounding volume for each link for 
self-collision avoidance. The arm will automatically 
avoid hitting any other link in the system without the 
operator’s attention. 

Operator Control Unit (OCU) With Multiple Views 

In one embodiment, an operator control unit can provide a 
user with multiple views, such as an on-the-ann camera view 
and a fish-eye view from a mobile platform. A user can select 
from different modes (drive, manipulate . . . etc.) to finish the 
goal. The arm may need to be driven differently when the 
camera on the arm is looking in different directions, in some 
cases. Through experiences drawn from human computer 
interfaces, often WYSIWYG (what-you-see-is-what you- 
get) can provide an intuitive way of connecting the human and 
the machine. It can be advantageous to provide the operator a 
way to control the arm with respect to an individual camera 
view (e.g., with respect to the local coordinate frame of that 
individual camera view). For example, upon a particular view 
being brought into focus (e.g., via view selection by a user), 
the user interface (e.g., the user interface software) can switch 
to the mode that is tied to that current view; a seamless mode 
change. The operator can give a desired end effector pose with 
respect to the current camera view; the software can translate 
the command to target joint angles automatically without the 
operator’s attention and/or input. The operator can place the 
3D cursor in each of the views exactly the same way as in any 
other view. 

For example, a viewer (e.g., Actin viewer) supports render- 
ing multiple views by defining multiple virtual cameras for 
simulation purposes. 

For simulating or controlling a robot with mobile base, it is 
advantageous in many cases to set up a camera view from the 
camera near the end of a robot arm or at an intermediate link 
along the arm. For example, we can set the arm camera to look 
at a gripper or other end effector, but it can also be set to look 
at any other part of the system. A typical camera may pan and 
tilt with respect to an arm link. Alternate cameras are often 
placed on the mobile base to provide a broader view of the 
robot anns or of the environment around the robot. In addi- 
tion, virtual cameras can be placed anywhere in the 3D simu- 
lation and control environment, allowing views of the simu- 
lated robot pose and control of the actual robot from any view 
angle. In some embodiments, actual cameras can also be 
used, either in addition to or in place of virtual cameras. 

The multiview approach can show multiple views of the 
robot on the OCU display. The multiview OCU can allow for 
direct control of the robot from any of the views, using a 
single active guide frame among all views or by using sepa- 
rate guide frames per view. Direct control can be done in the 
local coordinate frame of the selected window, making it 
intuitive and flexible. 



US 8,428,781 B2 


27 

Extensible Framework for User Input with Multiple 
Shared Views 

User input can be handled in a way that expands the ease of 
user input for all windows, including a main window as well 
as alternate windows, child windows, and/or sub windows. 
There is a new framework for registering input handlers, 
which allows for expansion and modification of the behavior 
of input events. A set of classes was created in the Actin 
toolkit to provide several levels of input control. These classes 
can be extended or overridden to create custom behavior. 

EcSGBaselnputFlandler — provides base class for frame- 
work to set and retrieve camera position and orientation. This 
class does not provide user input control ; only explicit camera 
placement is allowed with this class. This corresponds with 
the previous behavior of EcSGWindow. 

EcDefaultlnputElandler — A general class that takes user 
input events, such as from a mouse or keyboard or touch- 
screen and processes them for the current window. Example 
default behavior is left mouse button controls camera view 
(rotation and translation in x,y) and right mouse button 
manipulates the end-effector; middle mouse controls zoom- 
ing for camera or end-effector (z translation). This is the 
default input handler for EcBaseViewerMain Widget. 

Examples 3 & 4 

Free Base Kinematics Simulations 

These simulations involve free-base kinematics focusing 
on achievable robot configurations, independent of inertia 
involved in moving to a desired configuration. FIG. 14 shows 
an example 21 -joint (27-degree of freedom, including base 
motion) free-floating mechanism placing two frame end 
effectors and two point end effectors. The mechanism has 2 1 
joints and 4 end effectors and is being kinematically con- 
trolled. This simulated mechanism is built into code base for 
use in self-tests of the free-base code. It illustrates the ability 
of the toolkit to support a variety of complex mechanisms. 

FIGS. 15A-15D show the 57-link Robonaut model moving 
in free-base mode. FIGS. 15A-15D represent a sequence of 
images (from the same viewpoint) of the Robonaut model in 
free-base mode sequentially placing fingers of opposing 
hands. In free-base mode, all links on the manipulator are free 
to move. The only constraints are the end effectors. The 
toolkit can be used to support hand-over-hand movement of 
Robonaut when it is fielded. 

Example 5 
Dynamic Simulation 

This example involves dynamic simulations which include 
modeling of motion-related properties such as inertia. It illus- 
trates how two dynamic simulation methods for fixed bases, 
the Articulated Body Inertia Algorithm and the Composite 
Rigid-Body Inertia Algorithm, can be extended to support 
simulating free-base manipulators. Hie Order(N) Articulated 
Body Inertia algorithm is best for very large manipulators, 
while the Order(N 3 ) Composite Rigid-Body Inertia algo- 
rithm is best for smaller manipulators. 

Composite Rigid-Body Inertia Simulation Algorithm 

For fixed-base manipulators, dynamic simulation is imple- 
mented using an adaptation of the composite rigid-body algo- 
rithm for bifurcating manipulators. For more information on 
such algorithms, see M. W. Walker and D. E. Orin, “Efficient 
Dynamic Computer Simulation of Robotic Mechanisms,” 
Journal of Dynamic Systems, Measurement, and Control, 
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104, 205-211, 1982 and A. Fijany and A. K. Bejczy, “An 
Efficient Algorithm for Computation of Manipulator Inertia 
Matrix,” Journal of Robotic Systems, 7(1), 57-80, 1990, both 
of which are hereby incorporated by reference. 

5 This algorithm rims in Order(N 3 ) time, for N links 

The fixed-base composite rigid-body inertia algorithm 
uses the following dynamics equation: 

x =M(q)q+C(q)q+G(q)+B, (66) 

10 where x is the column vector of joint torques/forces, M(q) is 
the manipulator inertia matrix, q is the vector of joint position, 
C(q) represents the Coriolis forces, G(q) represents gravita- 
tional forces, and B represents the effect of external forces 
applied to the arm’s links. This equation, as shown, is only 

15 valid for a manipulator with a fixed base. When the base is 
free, it must be modified. 

For any frames i and j that are rigidly connected, LeUP 
be the cross-product matrix for y p lWj , the vector from the 
origin of frame i to the origin of frame j, expressed in frame j. 

20 And leUR, be the rotation matrix expressing frame i in frame 
j. Using this, let the matrices ^T t _j and A T t ■ be defined as 
follows: 
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-It?; 0 

J PjjRi -A’, 

(67) 


and 
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A Ti-> j = 

'At J Pj .i J R, 
0 J R; 

(68) 


35 


40 


45 


These transformations produce the following equalities: 

Fj^T^jF,, (69) 

and 

A^T^jA,. (70) 

For any rigid body, let j be the vector force applied to the 

link, ri be the moment, to be the angular velocity, ~v be the 

linear velocity, f a be an a priori external force applied to the 

body, n a be an a priori moment applied to the body, m be the 
mass, H be the cross-product matrix for the first moment of 
inertia, and J be the second moment of inertia. Then, the 
force/moment equations are given by the following: 
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J H r u> + fn yJl'n, +mv* - J (71) 

fi=Joi + oixJoi+Hv*-fi e . (72) 

Let the 6x6 rigid-body inertia be defined as follows: 


ml H 
H J 


( 73 ) 


And let a bias frame force be defined as 


B = 


wxH T 7o 
CO x Jco 


-fa 


(74) 
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With this, the rigid-body dynamics can be represented as 

F=I°A+B. (75) 

Effect of Base Acceleration on Joint Torque 

When the base link is free to move, the force on and 5 
acceleration of the base affects the manipulator dynamics. 
The affect of the base acceleration on any joint can be found 
by fixing all other joints and finding the torque required by the 
joint to sustain the acceleration on the composite rigid body 
outboard from the joint. This is illustrated in FIG. 16 , which 10 
shows how in this model the torque produced on joint i due to 
the acceleration of the base is the torque required to accelerate 
all the outboard links from the joint. This is an additive term, 
found by assuming all other joints are stationary. 

For the base joint, let the a priori external force and moment 15 

be divided into two components. Let f e an external force 
applied to the base, n e be an external moment applied to the 
body, f m be the force applied by child links to the base, and ., (| 
n m be the moment applied by child links . With this, j a = f e + 
f m and n = n e + n m . Then the bias force is given by 


ToxH t To - f * -f m 

ZtJ X Jco —n„ —n m 


The torque onjoint i due to acceleration A b can be found by 30 
assuming an otherwise stationary manipulator, with all other 
rates and accelerations zero. With this assumption, the base 
frame acceleration can be expressed in frame i using (70), the 
frame force required to move the outboard composite rigid 
body calculated using (75), and the joint torque calculated by 35 
taking the inner product of this force with ji,. This gives the 
torque onjoint i due to base acceleration as the following: 

xM, T ifLT b ~M b - (77) 

Let the matrix D be defined as follows: 40 


Effect Joint Accelerations on the Base Acceleration on 
Joint Torque 

When the base link is free to move, assuming an otherwise 
stationary manipulator, the force that must be exerted on 
composite rigid body i for acceleration only by joint i is given 

by 

F,=Ii% %• (81) 

Expressing this in the base frame, and changing the sign to 
represent the effective force applied to the base gives: 

b F,=- fTt-.trf+a. (82) 

Summing the contributions of all links and using the iden- 
tity Vi,j gives the following remarkable reuse 

of the matrix D to calculate the force F„, a applied to the base 
as a result of manipulator joint accelerations. 


F ma =-D T q. (83) 

Let the total force applied by the manipulator to the base be 


where F mc , F mg , and F me represent the force due to Coriolis 
and centripetal terms, gravity, and external forces, respec- 
tively. 

If F c represents the external forces applied to the base link 
directly, then (75) gives 

F ma +F=I b c A h -F m -F mg -F me , (85) 

where I 4 C is the composite rigid-body inertia of the entire 
manipulator, including the base. 

Substituting in (83) gives 

D T q+I b c A b =F,+F mc+ F mg+ F „ , (86) 

Combining this with (79) gives a new manipulator dynam- 
ics equation 


It 

D[q) T ' 

Ab F^e 4 ” Fmc 4 " F m g 4 - F me 

D(q) 

M(q) . 

q \ T — C{q)q - G{q) + B 


00 Iq(a 7)>-»o) 


This can be evaluated in code by calculating 1/ (|> ( as the 50 
frame force produced by unit acceleration of joint i with all 
other joints stationary (1/ is symmetric), then transforming 
this force to the base frame using the identity ^T,_ ,, = f^j 

ViJ. 

With this definition, the manipulator dynamics equation 55 
becomes 

T =M(q)q+C(q)q+G(q)+DA b +B. (79) 

Note that, because gravitational G(q) is explicit, A b is the 
seen (rather than felt) value. 60 

Changing Reference Frames for Matrix D 
Because D applies to the base acceleration, it has a frame of 
representation, just as the base acceleration does. The for- 
mula to change the frame of expression for D from j to i is 
given by the following (with ^T ( . defined through (68)): 65 


For N joint degrees of freedom, I* c is 6x6, D(q) isNx6, and 
M(q) is NxN. Solving this for A b and q and time integrating 
these gives the free-base composite rigid-body algorithm. In 
concatenating A b and q, A fe is placed on top of q to simplify the 
calculation of the Cholesky Decomposition of the left-hand 
matrix in (87). Note that this matrix, which must be inverted, 
is guaranteed not to be singular for a real system (otherwise 
acceleration could be achieved with no force). 

Solving for the Accelerations 
Let 


It D(q) 1 
D(q) M(q) 


F, + F~c + F mg 4- F m , 


T-C(q)q-G(q) + B _ 


With these definitions, (87) becomes 


‘D^DUT^j). 
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an (N+6)-dimensional fully constrained linear equation. 
Cholesky Decomposition (decomposition into a lower-trian- 
gular square root) is ideal for solving this for q because M(q), 
like M(q), is positive definite. In this approach, M(q) is 
decomposed as follows: 5 


M=LL Z ■ (92) 

with E lower triangular. 

The constness of l b c can be exploited in the calculation of 
E. Let I b c be decomposed as to 


I b c =LJL b r , (93) 

with L 4 lower triangular. L 4 is constant and only needs to be 
calculated once. 

Let E be the Nx6 matrix satisfying the following: 15 

L b E r =D T , (94) 

which can be solved using back substitution with L b , and let 
L M he defined such that 

M(q)-EE T =l,J. M T . (95) 2 ° 

This can be solved using Cholesky decomposition on an 
NxN matrix. 

With these values, E is evaluated as 
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Here, F is the 6x1 frame force that must be applied to the 
articulated body to achieve 6x1 frame acceleration A. (For 

vector force f , moment 11 , linear acceleration a*, and angu- 
lar acceleration a, represented as 3x1 column vectors, F= 

[ f T a T ] T and A=[ fUa r ] r .) The 6x1 frame force B is a bias 
force that is a function of external forces on the links, internal 
forces on the links, gravity, and link velocities. B represents 
all contributors to the force on the link except acceleration A. 
It is the force required to produce no acceleration of the 
handle. 

The iterative formulas to calculate I * and B are given by the 
following: 


i A - 
h — 


[if +i? - T \ iUj<P ) 


( 100 ) 

( 101 ) 


When the base is free, these equations can be continued to 
the base link. Then, at the base link, the frame acceleration 
will be given by 


U 0 ' 
E L m 


(96) 
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Using (73) and (93), 


yfm I 0 

U = , r- _ . 

1 /Vmtf Lj 

Where E j is lower triangular, and 

LjL T j = j- -hh t . 
m 


(97) 
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Articulated-Body Simulation Algorithm 

The articulated-body algorithm is used for simulation in 
the software toolkit, running in Order(N) time. More infor- 
mation on such an algorithm can be found in R. Featherstone, 
Robot Dynamics Algorithms, Kluwer Academic Publishers, 
Boston, 1987, and K. W. Lilly, Efficient Dynamic Simulation 
of Robotic Mechanisms, Kluwer Academic Publishers, Bos- 
ton, 1993, both of which are hereby incorporated by refer- 
ence. This algorithm allows fast simulation of very complex 
mechanisms. 

Articulated Body Inertia 

An articulated body is a collection of connected articulat- 
ing links, as illustrated in FIG. 17 . Any single rigid body 
within the articulated body can be used as a handle for defin- 
ing the relationship between force and acceleration. The 
articulated body in FIG. 17 is composed of one or more rigid 
links connected by articulated joints. One rigid link, in this 60 
case link i, is taken as the handle to define the relationship 
between 6'1 frame force (F) and 6'1 frame acceleration (A). 

Associated with any handle is a 6x6 articulated-body iner- 
tia J A that satisfies the following equation for any physically 
realizable frame acceleration (6x1) A: 


A b =(I„ A T\F b -B b ). (102) 

Here, A b is the acceleration that is felt, rather than seen. If 
the frame is stationary with respect to a gravitational field, 
then A b will show acceleration upward. 

Free-Base Dynamics Simulation 

The .Articulated-Body Inertia Algorithm and the Compos- 
ite Rigid-Body Inertia Algorithm were modified for floating- 
base manipulators as described above. Testing was done by 1 ) 
verifying the algorithms conserve energy and momentum and 
2) comparing the two independent algorithms for equality of 
results. FIGS. 18A-18D show side-by-side comparisons of 
the two algorithms simulating a free-base three-link mecha- 
nism. 

The teachings of U.S. Pat. No. 6,757,587, titled “METH- 
ODS AND APPARATUS FOR DYNAMICALLY REPRO- 
GRAMMING REMOTE AUTONOMOUS AGENTS” and 
issued on Jun. 29, 2004, are hereby incorporated by reference. 

One skilled in the art will appreciate further features and 
advantages based on the above-described embodiments. 
Accordingly, the claims are not to be limited by what has been 
particularly shown and described by way of example. The 
appended claims are incorporated by reference herein and are 
considered to represent part of the disclosure and detailed 
description of this patent application. 

All publications and references cited herein are expressly 
incorporated herein by reference in their entirety. 

What is claimed is: 

1 . A method of controlling a robotic apparatus comprising 
a plurality of movable elements coupled to one another via 
one or more j oints defining one or more degrees of freedom of 
movement for each of the movable elements, at least one of 
said movable elements comprising a mobile base for the 
apparatus, the control method comprising: 

receiving input signals indicating a desired position or 
motion constraint for at least one of the movable ele- 
ments; 

generating, based on the input signals, a plurality of control 
signals to control movement of the mobile base and at 
least one other movable element to achieve the desired 


f=i a a-b. 
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motion by optimizing a pre-defined scalar function of 
the motion and positions of the movable elements and 
the mobile base; 

applying the control signals to the mobile base and the at 
least one other movable element to concurrently move 5 
the mobile base and the at least one other movable ele- 
ment to effect the desired motion; 

wherein the pre-defined scalar function is defined by: 

]i= l Aq T Wq+aq T F, 10 

where q is a n-length vector representation of the positions 
of the one or more joints and the mobile base; with q 
being its time derivative (change in position over time); 

W is a matrix function W(q); F is a vector function of 
configuration; and a is a scalar function of configura- 15 
tion; and 

wherein W, F and a are selected to define a control strategy 
for controlling the robotic apparatus. 

2. The method of claim 1, wherein at least one of the 
movable elements comprises an end effector. 

3. The method of claim 2, wherein the end effector com- 
prises any of a gripper, a cutter, a saw, a haptic element, a 
multi-fingered end effector. 

4. The method of claim 1, wherein at least one of the 
movable elements provides contact with the environment for 25 
the purpose of locomotion. 

5. The method of claim 1, wherein the mobile base com- 
prises any of a wheel, a track, and a rail. 

6. The method of claim 1, wherein the mobile base com- 
prises any of a digging, climbing, and spelunking apparatus. 11 1 

7. The method of claim 1, wherein the mobile base com- 
prises any of a boat, submarine, and hovercraft. 

8. The method of claim 1, wherein the mobile base com- 
prises any of an airplane, helicopter, and air-floating vehicle. 

9. The method of claim 1, wherein the plurality of movable 
elements comprise a robotic arm and a mobile base. 

10. The method of claim 1, wherein the input signals are 
generated by a user input device. 

11. The method of claim 10, further comprising, (i) dis- 
playing a plurality of views of the robotic apparatus pose to a 411 
user, and (ii) allowing the userto control the robotic apparatus 
from each of the plurality of the views with the user input 
device. 

12. The method of claim 11, wherein the user input device 
requires only a single user action to switch control between 
the plurality of the views. 
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13. The method of claim 11, wherein the plurality of views 
comprise a plurality of rendered views. 

14. The method of claim 10, further comprising, (i) dis- 
playing a plurality of views of the robotic apparatus pose to a 
user, and (ii) allowing the userto control the robotic apparatus 
from each of the plurality of views with the user input device, 
and (iii) automatically updating each of the plurality of views 
based on a user’s control input in any one view. 

15. The method of claim 1, wherein the input signals are 
transmitted over any of audio signal, electromagnetic signal, 
or a computer network. 

16. The method of claim 1, wherein the mobile base has 
two or more degrees of freedom of movement comprising 
linear velocity and angular velocity. 

17. The method of claim 1, comprising applying the control 
signals to one or more actuators coupled to the one or more 
movable elements to effect the desired motion. 

18. The method of claim 1, wherein the pre-defined scalar 
function is associated with movement of the robotic appara- 
tus, a gradient of potential energy associated with movement 
of the robotic apparatus, singularity avoidance, accuracy, 
joint-limit avoidance, obstacle avoidance. 

19. The method of claim 1, wherein the pre-defined scalar 
function signifies a constraint on movement of at least one 
movable element. 

20. The method of claim 1, wherein the movable elements 
of the robotic apparatus exhibit, collectively, at least seven 
degrees of freedom of movement. 

21 . The method of claim 1 , wherein generating the control 
signals comprises determining control vectors using the fol- 
lowing equation: 


)ji-r f-vi 

[NjW J [-ctNjFl 

where q is a n-length vector representation of the positions 
of the one or more joints and the mobile base; with q 
being its time derivative (change in position over time); 
Visa m-length vector representation of desired motion 
of at least one of the plurality of movable elements; J is 
an (mxn) manipulator Jacobian; N/Ts an nx(n-m) set of 
vectors defined such that JN/=0; W is a matrix function 
of q; F is a vector function of q; T is a matrix transpose 
operator; and a is a scalar function of q. 





